/*
 * @Description: 
 * @Author: diyhome
 * @Date: 2021-01-01 6:42
 * @File: mpu6050_cpp
*/
static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};
#include <cmath>
#include "mpu6050_cpp.h"
#include "mpu6050.h"
#include "eMPL/inv_mpu.h"
#include "../mcu_std/mcu_std.h"
#include "eMPL/inv_mpu_dmp_motion_driver.h"

using mcu_std::endl;

#define q30  1073741824.0f

mpu6050_hal::mpu6050_cpp::mpu6050_cpp() {
    MPU_Init();
}

/**
 * 初始化的时候设置陀螺仪与加速度计的量程
 * @param accel 加速度量程
 * @param gyro 陀螺仪量程
 */
mpu6050_hal::mpu6050_cpp::mpu6050_cpp(settings accel, settings gyro) {
    MPU_Init();
    MPU_Set_Accel_Fsr(static_cast<uint8_t>(accel));
    MPU_Set_Gyro_Fsr(static_cast<uint8_t>(gyro));
}

float mpu6050_hal::mpu6050_cpp::get_temperature() {
    float temperature = MPU_Get_Temperature();
    return temperature;
}

void mpu6050_hal::mpu6050_cpp::get_acceleration() {
    MPU_Get_Accelerometer(&(this->ax),&(this->ay),&(this->az));
}

void mpu6050_hal::mpu6050_cpp::get_gyro_speed() {
    MPU_Get_Gyroscope(&(this->gx),&(this->gy),&(this->gz));
}

mpu6050_hal::mpu6050_dmp::mpu6050_dmp() {
    if(!mpu_init()) {
        if(!mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)) cout << "mpu_set_sensors complete"<< endl;
        if(!mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)) cout<<"mpu_configure_fifo complete"<<endl;
        if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) cout<<"mpu_set_sample_rate complete"<<endl;
        if(!dmp_load_motion_driver_firmware()) cout<<"dmp_load_motion_driver_firmware complete"<<endl;
        if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) cout<<"dmp_set_orientation complete"<<endl;
        if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                               DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                               DMP_FEATURE_GYRO_CAL)) cout<<"dmp_enable_feature complete"<<endl;
        if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) cout<<"dmp_enable_feature complete"<<endl;
        run_self_test();
        if(!mpu_set_dmp_state(1)) cout<<"mpu_set_dmp_state complete"<<endl;
    }
}

uint8_t mpu6050_hal::mpu6050_dmp::read_dmp() {
    short gyro[3], accel[3], sensors;
    float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    unsigned long sensor_timestamp;
    unsigned char more;
    long quat[4];
    if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return 1;
    if (sensors & INV_WXYZ_QUAT)
    {
        q0=quat[0] / q30;
        q1=quat[1] / q30;
        q2=quat[2] / q30;
        q3=quat[3] / q30;
        this->pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
        this->roll= (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
        this->yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
        return 0;
    }
    else
        return 2;
}

float mpu6050_hal::mpu6050_dmp::get_temperature() {
    float temperature = MPU_Get_Temperature();
    return temperature;
}
